#!/usr/bin/env python
# -*- coding: UTF-8 -*-
import cv2
import numpy as np
import sys
import math
# 8.26
# 这份代码用来进行巡线图片测试

# 透视变换用到的参数
global src_set
global dst_set
global M_Perspective
global image_HEIGHT
global image_WID
# --------------------
# 二值化形态形态学操作用到的卷积核
global kernel_set
# --------------------
# 进行巡线检测用到的参数
global mid_point

global left_boundary_40, left_flag_40
global right_boundary_40, right_flag_40
global left_boundary_250, left_flag_250
global right_boundary_250, right_flag_250

global left_boundary_345, left_flag_345
global left_boundary_474, left_flag_474
global right_boundary_345, right_flag_345
global right_boundary_474, right_flag_474

global Extra_lines
global White_points_dec_thresholds


# 进行参数初始化的函数
def init_lane_detect():
    # 透视变换用到的参数
    global src_set
    global dst_set
    global M_Perspective
    global image_HEIGHT
    global image_WID
    # --------------------
    # 二值化形态形态学操作用到的卷积核
    global kernel_set
    # --------------------
    # 进行巡线检测用到的参数
    global mid_point  # 图像的中线位置
    # 各个行的左右边界，以及左右点的计数标志，就是检测到一个边界点，计数标志就+1，直到阈值
    global left_boundary_40, left_flag_40
    global right_boundary_40, right_flag_40

    global left_boundary_250, left_flag_250
    global right_boundary_250, right_flag_250

    global left_boundary_345, left_flag_345
    global left_boundary_474, left_flag_474
    global right_boundary_345, right_flag_345
    global right_boundary_474, right_flag_474
    # 判断单线还是双线的时候，扫描的额外行数
    global Extra_lines
    # 判断边线边界的白色点检测阈值
    global White_points_dec_thresholds

    # ----------------------------------------------------------------------------透视变换需要用到的参数设置
    image_HEIGHT = 480
    image_WID = 640
    # 这里获取透视变换的矩阵
    # 首先需要设定好两组四个需要标定的点
    # 第一组目标点，需要你打开原图，找到四个你需要进行变换的坐标点，你有写好测试程序路径为/调试工具/Find the coordinates of the picture.py
    src_set = np.float32([[205, 219], [424, 219], [27, 340], [598, 344]])

    # 第二组标定点就比较简单，和上面你标定的点是对应的，你这里采用了Z字型的标定顺序
    dst_set = np.float32([[80, 0], [560, 0], [80, 480], [560, 480]])
    # 这里通过调用一个API来获得变换矩阵
    M_Perspective = cv2.getPerspectiveTransform(src=src_set, dst=dst_set)

    # -------------------------------------------------------------------------------形态学操作的矩阵
    # 生成用于形态学操作的矩阵
    kernel_set = cv2.getStructuringElement(shape=cv2.MORPH_RECT,
                                           ksize=(7, 7),
                                           anchor=(-1, -1)
                                           )

    # -------------------------------------------------------------------------------巡线参数设

    mid_point = 320
    # 都初始化为0
    left_boundary_40 = 0
    left_flag_40 = 0

    left_boundary_250 = 0
    left_flag_250 = 0

    left_boundary_345 = 0
    left_flag_345 = 0

    left_boundary_474 = 0
    left_flag_474 = 0

    right_boundary_40 = 0
    right_flag_40 = 0

    right_boundary_250 = 0
    right_flag_250 = 0

    right_boundary_345 = 0
    right_flag_345 = 0

    right_boundary_474 = 0
    right_flag_474 = 0

    # 额外扫描行数设置
    Extra_lines = 5

    # 判断边线边界的白色点检测阈值设为10，检测到10个点就认为这个是巡线的边界
    White_points_dec_thresholds = 10


# 对赛道进行透视变换获得俯视图的函数
def warp_image(image_for_warp):
    global src_set
    global dst_set
    global M_Perspective
    global image_HEIGHT
    global image_WID

    # 下面是进行透视变换的操作
    wig_perspective_get = cv2.warpPerspective(src=image_for_warp,
                                              M=M_Perspective,
                                              dsize=(image_WID, image_HEIGHT)
                                              )

    # 返回透视变换的图片
    return wig_perspective_get


# 对图像进行二值化操作的函数(这里面包含了形态学的开操作来去除噪点)
def Binary_image(img_bina):
    global kernel_set

    Bina_image = img_bina
    # 将图像变为灰度图
    wig_Gray = cv2.cvtColor(Bina_image, cv2.COLOR_BGRA2GRAY)

    # 用threshold（手动/自动设置全局阈值）对灰度图进行二值化处理
    ret, wig_Bina_set = cv2.threshold(src=wig_Gray,
                                      thresh=130,
                                      maxval=255,
                                      # 这里设计这个参数，其实是让函数自己找一个全局阈值了
                                      type=cv2.THRESH_BINARY | cv2.THRESH_OTSU)

    # 对获得的图像进行形态学的开操作，去除黑色区域的白色噪点
    open_cal_get = cv2.morphologyEx(src=wig_Bina_set,
                                    op=cv2.MORPH_OPEN,
                                    kernel=kernel_set,
                                    iterations=2)

    return open_cal_get


# 找到线的道路线的大概边界
def boundary_detect(bina_img_for_lane_detect):
    # 进行巡线检测用到的参数
    global mid_point
    # 各个行的左右边界，以及左右点的计数标志，就是检测到一个边界点，计数标志就+1，直到阈值
    global left_boundary_40, left_flag_40
    global right_boundary_40, right_flag_40

    global left_boundary_250, left_flag_250
    global right_boundary_250, right_flag_250

    global left_boundary_345, left_flag_345
    global right_boundary_345, right_flag_345

    global left_boundary_474, left_flag_474
    global right_boundary_474, right_flag_474
    # 判断单线还是双线的时候，扫描的额外行数
    global Extra_lines
    # 判断边线边界的白色点检测阈值
    global White_points_dec_thresholds

    # 参数都初始化为0
    left_boundary_40 = 0
    left_flag_40 = 0

    left_boundary_250 = 0
    left_flag_250 = 0

    left_boundary_345 = 0
    left_flag_345 = 0

    left_boundary_474 = 0
    left_flag_474 = 0

    right_boundary_40 = 0
    right_flag_40 = 0

    right_boundary_250 = 0
    right_flag_250 = 0

    right_boundary_345 = 0
    right_flag_345 = 0

    right_boundary_474 = 0
    right_flag_474 = 0

    # 这里拷贝一份图片，用于扫描处理
    img_detect_line = bina_img_for_lane_detect

    # 先判断是单线模式还是双线模式
    # 这里对拿到的二值化图像进行扫描，行数如下    # 415 345

    # 从中间往两边进行扫描
    for i in range(mid_point):

        # 行的扫描

        # 向左扫描
        # 扫描40行+额外的(可以设定)行数
        if 255 in img_detect_line[40:40 + Extra_lines, mid_point - i]:
            left_flag_40 += 1

        # 扫描250行+额外的(可以设定)行数
        if 255 in img_detect_line[250:250 + Extra_lines, mid_point - i]:
            left_flag_250 += 1

        # 扫描345行+额外的(可以设定)行数
        if 255 in img_detect_line[345:345 + Extra_lines, mid_point - i]:
            # # 这里是将扫描的行涂黑
            # img_detect_line[345:345+Extra_lines, mid_point - i] = 255
            left_flag_345 += 1
        # 扫描474行+额外的5行
        if 255 in img_detect_line[474:474 + 5, mid_point - i]:
            # # 这里是将扫描的行涂黑
            # img_detect_line[474:474+5, mid_point - i] = 255
            left_flag_474 += 1

        # 向右扫描
        # 扫描40行+额外的(可以设定)行数
        if 255 in img_detect_line[40:40 + Extra_lines, mid_point + i]:
            right_flag_40 += 1

        # 扫描250行+额外的(可以设定)行数
        if 255 in img_detect_line[250:250 + Extra_lines, mid_point + i]:
            right_flag_250 += 1

        # 扫描345行+额外的行数
        if 255 in img_detect_line[345:345 + Extra_lines, mid_point + i]:
            # # 这里是将扫描的行涂黑
            # img_detect_line[345:345+Extra_lines, mid_point + i] = 0
            right_flag_345 += 1

        # 扫描474行+额外的5行
        if 255 in img_detect_line[474:474 + 5, mid_point + i]:
            # # 这里是将扫描的行涂黑
            # img_detect_line[474:474+5, mid_point + i] = 0
            right_flag_474 += 1

        # ---------------------------------------------------------------------行标志的判断
        # 判断白色点的检测标志是否达到阈值
        # ---------------左边界
        # 40行左边界判断
        if left_flag_40 == White_points_dec_thresholds:
            left_boundary_40 = mid_point - i
        if (left_flag_40 < White_points_dec_thresholds) and i == 319:
            left_boundary_40 = 0

        # 250行左边界判断
        if left_flag_250 == White_points_dec_thresholds:
            left_boundary_250 = mid_point - i
        if (left_flag_250 < White_points_dec_thresholds) and i == 319:
            left_boundary_250 = 0

        # 345行左边界的判断
        if left_flag_345 == White_points_dec_thresholds:
            # 达到阈值，就设定为这一行的边界，这里获得的值是边界的x坐标，就是边界处于第几列
            left_boundary_345 = mid_point - i
        # 没有达到阈值，但是到达图像边缘，那么将左边界设为0
        if (left_flag_345 < White_points_dec_thresholds) and i == 319:
            left_boundary_345 = 0

        # 474行左边界的判断
        if left_flag_474 == White_points_dec_thresholds:
            # 达到阈值，就设定为这一行的边界，这里获得的值是边界的x坐标，就是边界处于第几列
            left_boundary_474 = mid_point - i
        # 没有达到阈值，但是到达图像边缘，那么将左边界设为0
        if (left_flag_474 < White_points_dec_thresholds) and i == 319:
            left_boundary_474 = 0
        # --------------------------------右边界
        # 40行右边界判断
        if right_flag_40 == White_points_dec_thresholds:
            right_boundary_40 = mid_point + i
        if (right_flag_40 < White_points_dec_thresholds) and i == 319:
            right_boundary_40 = 640

        # 250行右边界判断
        if right_flag_250 == White_points_dec_thresholds:
            right_boundary_250 = mid_point + i
        if (right_flag_250 < White_points_dec_thresholds) and i == 319:
            right_boundary_250 = 640

        # 345行右边界的判断
        if right_flag_345 == White_points_dec_thresholds:
            # 达到阈值，就设定为这一行的边界，这里获得的值是边界的x坐标，就是边界处于第几列
            right_boundary_345 = mid_point + i
        # 没有达到阈值，但是到达图像边缘，那么将左边界设为0
        if (right_flag_345 < White_points_dec_thresholds) and i == 319:
            right_boundary_345 = 640
        # 474行右边界的判断
        if right_flag_474 == White_points_dec_thresholds:
            # 达到阈值，就设定为这一行的边界，这里获得的值是边界的x坐标，就是边界处于第几列
            right_boundary_474 = mid_point + i
        # 没有达到阈值，但是到达图像边缘，那么将左边界设为0
        if (right_flag_474 < White_points_dec_thresholds) and i == 319:
            right_boundary_474 = 640

    return left_boundary_40, right_boundary_40, left_boundary_250, right_boundary_250, left_boundary_345, right_boundary_345, left_boundary_474, right_boundary_474


# 计算出巡线模式
def get_line_mode(lef_bound_40, rig_bound_40, lef_bound_250, rig_bound_250, lef_bound_345, rig_bound_345, lef_bound_474,
                  rig_bound_474):
    # 250行
    # 345行
    # 474行
    # 条件解释：三行的双边扫描不能都到到图片边界，而且两条边界的距离必须大于150

    # 双线模式
    if (lef_bound_250 != 0 and rig_bound_250 != 640 and abs(lef_bound_250 - rig_bound_250 > 150)) or (
            lef_bound_345 != 0 and rig_bound_345 != 640 and abs(lef_bound_345-rig_bound_345) > 150) or (
            lef_bound_474 != 0 and rig_bound_474 != 640 and abs(lef_bound_474-rig_bound_474) > 150):
        print('Both Line')
        return 2

    # 40行 (有边界处于中间)
    # 250行
    # 左边没到边界，右边到边界
    # 右边线模式
    if ((lef_bound_40 != 0 and rig_bound_40 == 640) or rig_bound_40 < 350) and (lef_bound_250 != 0 and rig_bound_250 == 640):
        print('Single_turn_right_line')
        return 3


# 通过边界算出转角
def get_angle(lef_bound_250, rig_bound_250, lef_bound_345, rig_bound_345, lef_bound_474,
                  rig_bound_474, image_bina):
    point_345 = (int((lef_bound_345 + rig_bound_345)/2), 345)
    point_250 = (int((lef_bound_250 + rig_bound_250)/2), 250)
    point_474 = (int((lef_bound_474 + rig_bound_474)/2), 474)
    # 将直线画出来
    cv2.line(img=image_bina,
             pt1=point_345,
             pt2=point_250,
             color=255,
             thickness=1,
             lineType=cv2.LINE_AA,
             shift=0
             )
    cv2.line(img=image_bina,
             pt1=point_474,
             pt2=point_345,
             color=255,
             thickness=1,
             lineType=cv2.LINE_AA,
             shift=0
             )

    # k_345_250 = float((float(point_250[0]) - float(point_345[0])) / (-(float(point_250[1]) - float(point_345[1]))))
    # print('k_345_250:', k_345_250)
    # angle_345_250 = (math.atan(k_345_250) / (2 * np.pi)) * 360
    # print('angle_345_250:', angle_345_250)
    k_474_345 = float((float(point_345[0]) - float(point_474[0])) / (-(float(point_345[1]) - float(point_474[1]))))
    print('k_474_345:', k_474_345)
    angle_474_345 = (math.atan(k_474_345) / (2 * np.pi)) * 360
    return angle_474_345

if __name__ == '__main__':
    # 获取到摄像头帧数
    cap_camera_wig = cv2.VideoCapture(0, cv2.CAP_DSHOW)  # cv_4.2.1.30_cp38
    # 进行参数初始化操作
    init_lane_detect()
    while True:
        # 读取图片
        ret1, frame1 = cap_camera_wig.read()
        # 如果读取到了图片
        if ret1:
            # 进行畸变的矫正
            # camera_get = Straighten_the_camera(frame1)
            # 采用无畸变摄像头就不用进行矫正
            camera_get = frame1
        # 没有读取到就退出进程
        else:
            print("can't get the pic")
            sys.exit()

        # -------------------------------------------------
        # 读取图片
        image_test = camera_get
        # 对图片进行透视变换
        image_warp = warp_image(image_test)
        # 对图片进行二值化处理
        image_bina = Binary_image(image_warp)

        # ---------------------------   边界的相关测试
        left_bound_40, right_bound_40, left_bound_250, right_bound_250, left_bound_345, right_bound_345, left_bound_474, right_bound_474 = boundary_detect(
            image_bina)

        get_angle(left_bound_250, right_bound_250, left_bound_345, right_bound_345, left_bound_474, right_bound_474, image_bina)
        # get_line_mode(left_bound_40, right_bound_40, left_bound_250, right_bound_250, left_bound_345, right_bound_345,
        #               left_bound_474, right_bound_474)

        # print('left_boundary_40:', left_bound_40)
        # print('right_boundary_40:', right_bound_40)
        # print('left_boundary_250:', left_bound_250)
        # print('right_boundary_250:', right_bound_250)
        # print('left_boundary_345:', left_boundary_345)
        # print('right_boundary_345:', right_boundary_345)
        # print('left_boundary_474:', left_bound_474)
        # print('right_boundary_474:', right_bound_474)

        # ---------------------------

        # 显示图片
        cv2.imshow('test_image', image_bina)
        # 等待退出指令
        key_wig = cv2.waitKey(1)

        if (key_wig & 0xFF) == ord('q'):
            print('exit')
            break

    # 释放视频资源
    cap_camera_wig.release()
    # 释放窗口
    cv2.destroyAllWindows()
